#include "Qbot.h"

#define foreach_wheel(n) for(int n=0;n<2;n++)
#define FrameHead 0x5A
#define FrameHeadLen 3U
#define FrameMaxId   20U                       //目前ID用到了15，留4条预留
#define MODBUS_SLAVE_ID 1U

/* UART驱动 */
extern ARM_DRIVER_USART Driver_USART1, Driver_USART2, Driver_USART3, Driver_USART4, Driver_USART5;
extern USART_INFO USART1_Info, USART2_Info, USART3_Info, USART4_Info, USART5_Info;

/* USB */
static Stream usb(&USART1_Info, &Driver_USART1);
/* RS485 */
static Stream stream2(&USART2_Info, &Driver_USART2);
static Stream stream3(&USART3_Info, &Driver_USART3);
/* RS232 */
static Stream rs232m(&USART4_Info, &Driver_USART4);         //公头
static Stream rs232f(&USART5_Info, &Driver_USART5);         //母头
/* Wingbot协议 */
Simple_dataframe dataframe(&rs232f);
/* Modbus从站 */
ModbusSerial mb_serial;

/* 协议解析相关 */
static u8 usb_buf[512] = {0};
static u8* parse_buf = (u8*)usb_buf;
static u8 usb_frame_head[FrameHeadLen];        //帧头预读取

/* 电机, modbus站号分别是1和2 */
KincoMotor motor2(2, &stream2);
KincoMotor motor1(1, &stream2);

/* 编码器, 站号3 */
Briter briter_econder(3, &stream3);

uint32_t frame_cnt = 0;
uint32_t receive_cnt = 0;

Qbot::Qbot()
{
    this->frame = &dataframe;
    dh = Data_holder::get();
    st = MySetting::get();

    wheel_vel = 0;
    motor[0] = &motor1;
    vl_dire = st->motor_direction[1];
    motor[1] = &motor2;
    vr_dire = st->motor_direction[2];
    state = WHEEL_NORMAL;
    encoder = &briter_econder;
    encoder_total_turns = 0.0;
}

void Qbot::init()
{
    /* 开启USART定时器 */
    start_usart_timer();
    /* 设定10ms定时器 */
    set_ms_timer(TIM3, 10);
    /* 设定100ms定时器 */
    set_ms_timer(TIM4, 100);
    Led::get()->init();
    usb.begin(921600);
    /* 初始化485读写控制GPIO */
    RS485EN_init();
    can_mode_init(CAN_Mode_Normal, 500);
    stream2.begin(115200);
    stream3.begin(115200);
    rs232f.begin(115200);
    modbus_serial_init();
    
    foreach_wheel(i)
    {
        if(motor[i]->initModbus())
        {
            motor[i]->powerOnOff(true);
            dh->robot_state.wheels[i].ready = 1U;
        }
    }
    if(encoder->initModbus())
    {
        dh->robot_state.encoder.ready = 1U;
    }
    
    this->frame->register_notify(ID_SET_VELOCITY, this);
    this->frame->register_notify(ID_SET_EACH_MOTOR, this);
    this->frame->register_notify(ID_RESET_WHEELS, this);
    this->frame->register_notify(ID_CLEAR_ODOM, this);
    this->frame->register_notify(ID_SET_ODOM_TF, this);
    this->frame->register_notify(ID_SET_POWER, this);
    this->dh = Data_holder::get();
    
    /* 两秒的看门狗 */
    //wdg_init();
}

void Qbot::modbus_serial_init()
{
    mb_serial.config(&rs232m, 115200, NULL);
    mb_serial.setSlaveId(MODBUS_SLAVE_ID);
    /* add holding registers */
    mb_serial.addHreg(0x0000, 0x0000);
}

void Qbot::run_forever()
{
    while(1)
    {
        if(time_flags.ms100)ms100_loop();
        if(time_flags.ms10)ms10_loop();
        main_loop();
    }
}

extern Queue_T usart1_rx_queue;
void Qbot::main_loop()
{
    iwdg_feed();        //喂狗
    if(rs232f.dataReady())
    {
        int len = rs232f.available();       //总长度
        int read_len = 0;                //已读的部分
        int left_len = len;              //剩下的部分
        int frame_data_len = 0;          //单帧数据部分的长度

        while(left_len >= 5)
        {
            rs232f.readBytes(usb_frame_head, 1);
            if(usb_frame_head[0]==FrameHead)
            {
                rs232f.readBytes(&usb_frame_head[1], 1);
                if(usb_frame_head[1]<FrameMaxId)
                {
                    rs232f.readBytes(&usb_frame_head[2], 1);
                    frame_data_len = usb_frame_head[2] + 2;                //数据帧+CRC+帧尾
                    rs232f.readBytes(&usb_buf[FrameHeadLen], frame_data_len); //读队列数据
                    memcpy(usb_buf, usb_frame_head, FrameHeadLen);
                    read_len = frame_data_len + FrameHeadLen;              //单帧总长度
                    if(dataframe.data_recv(usb_buf, read_len))
                    {
                        dataframe.data_parse();
                        Led::get()->toggle();
                        frame_cnt++;
                    }
                }
                else
                    read_len = 2;
            }
            else
                read_len = 1;
            
            receive_cnt += read_len;
            left_len = left_len - read_len;
        }
        
        memset(usb_buf, 0, sizeof(usb_buf));
        //rs232f.clear_rx_buf();
    }
    
    modbus_serial_loop();

}

void Qbot::modbus_serial_loop()
{
    mb_serial.task();
    mb_serial.Hreg(0x0000, frame_cnt);
}

void Qbot::ms10_loop()
{
    time_flag_refresh(time_flags.ms10);
    if(state == WHEEL_NORMAL)
    {
        updateVelocity(wheel_vel*vl_dire, wheel_vel*vr_dire);
        getActualState();
        updateEncoder();
    }
}

void Qbot::ms100_loop()
{
    static uint32_t last_frame_cnt = 0;
    static int tcnt = 0;
    time_flag_refresh(time_flags.ms100);
    updateConnectState();
    if(last_frame_cnt == frame_cnt)
    {
        //如果500ms内frame_cnt不变说明没有数据在收发，按固定频率闪灯
        if((tcnt++)>5){
            Led::get()->toggle();
            tcnt = 0;
        }
    }
    else
        tcnt = 0;
    last_frame_cnt = frame_cnt;
}

void Qbot::update(const MESSAGE_ID id, void* data)
{
    switch (id){
    case ID_CLEAR_ODOM:
        ;
        break;
    case ID_SET_VELOCITY:
        this->setState(WHEEL_NORMAL);
        this->updateVelocity();
        break;
    case ID_SET_EACH_MOTOR:
//        foreach_wheel(i){
//            this->wheel[i]->updateVelocity(dh->each_motor.un.wheels[i].vL,dh->each_motor.un.wheels[i].vR);
//        }
        break;
    case ID_RESET_WHEELS:
        ;
        break;
    case ID_SET_ODOM_TF:
        //set_odom_tf(&dh->odom_tf);
        break;
    case ID_SET_POWER:
        foreach_wheel(i){
            this->motor[i]->powerOnOff(dh->power_onoff);
        }
        break;
    default:
        break;
    }
}

/**
* @brief 仅更新wheel_vel
*/
void Qbot::updateVelocity()
{
    float linear_x = dh->velocity.v_liner_x/1000.0f;
    short vx = mps2rpm(linear_x);
    wheel_vel = vx;
}

/**
* @brief 直接更新电机
*/
void Qbot::updateVelocity(short vl, short vr)
{
    motor[0]->setVeloctiy(vl);
    motor[1]->setVeloctiy(vr);
}

/**
* @brief 获取真实转速
*/
void Qbot::getActualState()
{
    static uint8_t first_run[2] = {1, 1};
    foreach_wheel(i)
    {
        dh->robot_state.wheels[i].actual_rpm = motor[i]->getMotorActualVel();
        if(motor[i]->getMotorActualPos())
        {
            double current_turn = motor[i]->actual_pos / EncoderResolution;
            double d_turn = current_turn - motor[i]->last_turns;
            if(abs(d_turn) > 10000.0)
            {
                if(d_turn > 0)d_turn -= KincoMaxTurn;
                else d_turn += KincoMaxTurn;
            }
            if(first_run[i])
            {
                first_run[i] = 0;
                motor[i]->init_turns = current_turn;
            }
            else{
                dh->robot_state.wheels[i].turns += d_turn;
            }
            motor[i]->last_turns = current_turn;
        }
    }
}

/**
* @brief 刷新传感器连接状态
*/
void Qbot::updateConnectState()
{
    foreach_wheel(i)dh->robot_state.wheels[i].ready = motor[i]->connectOnce();
    dh->robot_state.encoder.ready = encoder->connectOnce();
}

/**
* @brief rpm转速转换为米每秒
*/
float Qbot::rpm2mps(short rpm)
{
    return (rpm/60.0*PI*st->wheel_diameter) * st->ratio;
}

/**
* @brief 刷新角度编码器
*/
void Qbot::updateEncoder()
{
    static double last_turn = 0.0;
    static double current_turn = 0.0;
    static bool first_run = true;
    double dturn = 0.0;
    if(encoder->getTurns())
    {
        current_turn = encoder->getFloatTurn();
        dturn = current_turn - last_turn;
        if(abs(dturn) > st->max_d_turn)
        {   /* 如果变化量超过理论最大变化量，说明圈数溢出了 */
            if(dturn > 0)dturn -= BRT_MAX_TURN;
            else dturn += BRT_MAX_TURN;
        }

        if(!first_run)
        {
            this->encoder_total_turns += dturn;
            dh->robot_state.encoder.turns = this->encoder_total_turns;
            dh->robot_state.encoder.odom = this->encoder_total_turns * PI * st->encoder_wheel_d * st->encoder_direction;
        }
        else
            first_run = false;
        last_turn = current_turn;
    }
}

/**
* @brief 米每秒转换为rpm, 最大速度1.5m/s
*/
short Qbot::mps2rpm(float mps)
{
    if(mps<=1.5)
        return (short)(mps/st->ratio/(PI*st->wheel_diameter)*60);
    else
        return 0;
}

/* EOF */
